{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 动态系统建模\n",
    "\n",
    "【描述】\n",
    "\n",
    "Drake使用类似Simulink的方式描述动态系统。\n",
    "\n",
    "包括基本的模块（加法器、积分器、延迟等等），也包括机械系统的物理模型和逐渐增加的传感器、驱动器、控制器、规划器和估计器。\n",
    "\n",
    "所有的动力学系统都从drake::systems::System基类集成而来，而且必须显示声明drake::systems::State，drake::systems::Parameters和噪声/感染输入.\n",
    "drake::systems::Diagram类容许使用模块库对复杂系统进行建模。\n",
    "\n",
    "【模块】\n",
    "\n",
    "- 系统事件\n",
    "\n",
    "描述Drake系统如何对改变（时间、状态和输入）进行相应（通过事件）。\n",
    "\n",
    "- 基本单元\n",
    "\n",
    "一般的系统元素，如增益、乘法器、积分器和线性系统\n",
    "\n",
    "- 控制器\n",
    "\n",
    "- 估计器\n",
    "\n",
    "- 传感器\n",
    "\n",
    "- 操作器\n",
    "\n",
    "对机器人灵巧操作的系统建模\n",
    "\n",
    "- 信息传递\n",
    "\n",
    "- 多体系统\n",
    "\n",
    "与多体平台有关的系统模型\n",
    "\n",
    "- 感知\n",
    "\n",
    "- 离散系统\n",
    "\n",
    "- 随机系统\n",
    "\n",
    "- 可视化\n",
    "\n",
    "连接外部可视化工具的系统\n",
    "\n",
    "- 例子\n",
    "\n",
    "【类】\n",
    "\n",
    "class  SceneGraph< T >\n",
    "场景图类用于在框图中连接所有几何体（以及与几何体相关的操作）。\n",
    "\n",
    "class  MultibodyPlant< T >\n",
    "多体平台类是Drake为由一系列连接的物体组成的物理系统建模而提供的框架。\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1、系统事件\n",
    "\n",
    "简单动态系统的状态，如常微分方程$\\dot{x}=x$，可以根据事件使用前向数值积分进行推演。\n",
    "更复杂的系统需要更复杂的状态更新机制。\n",
    "我们称状态更新的工作为“事件”，引起事件的条件为“激发”，其中计算更新的机制为“处理器”。\n",
    "Simulator类的文档中，对相关的技术过程有详细的描述。\n",
    "\n",
    "- 说明\n",
    "\n",
    "- 事件和激发的类型\n",
    "\n",
    "- 事件如何被处理\n",
    "\n",
    "动态系统中，状态按事件演化。\n",
    "动态系统被仿真时，Drake的仿真器或其他求解器负责检测事件何时被激发，并分配何时的处理器函数，以更新状态随时间的变化。\n",
    "更新函数只会修改状态的拷贝，因此类中的每个更新函数会看到同样的更新前状态。。。\n",
    "\n",
    "时间也可以手动分配处理函数。\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2、基本单元\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 6、操作器\n",
    "\n",
    "支持机器人学灵巧操作的系统实现\n",
    "\n",
    "【类】\n",
    "\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 7、信息传递\n",
    "\n",
    "向流行的消息传递系统进行发布和订阅操作\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 8、多体系统\n",
    "\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 12、可视化\n",
    "\n",
    "【描述】\n",
    "\n",
    "可连接MeshCat\n",
    "可使用matplotlib显示平面刚体系统\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 13、例子\n",
    "\n",
    "【描述】\n",
    "\n",
    "例子中包括一些有用的系统实现。\n",
    "\n",
    "【模块】\n",
    "\n",
    "- Acrobot\n",
    "\n",
    "- 操作台\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 14、场景图\n",
    "\n",
    "【描述】\n",
    "\n",
    "场景图提供了注册几何体、基于当前上下文更新几何体的位置，并执行几何。。\n",
    "\n",
    "只有注册的“几何源”可以在场景图中引入几何体。\n",
    "几何源通常是其他叶子系统，不过。。。\n",
    "对于动态几何体（几何体的位形依赖于上下文），几何源必须提供其中所有几何体的位形信息。\n",
    "对于N个几何体源，场景图实例将具有N个位形输入端口。\n",
    "\n",
    "使用场景图的基本工作流程如下：\n",
    "\n",
    "- 注册为一个几何源，获取独立的源id\n",
    "- 注册几何体\n",
    "- 将源的几何输出端口与场景图的输入端口相连\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 14.1 输入\n",
    "\n",
    "。。。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 14.2 输出\n",
    "\n",
    "场景图有两个输出端口：\n",
    "\n",
    "- 查询端口\n",
    "\n",
    "\n",
    "- lcm可视化端口"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 14.3 使用场景图\n",
    "\n",
    "叶子系统关联场景图有两种方法：\n",
    "作为用户执行查询，或作为生产者将几何体引入共享空间，并定义与上下文有关的运动学数值。\n",
    "\n",
    "- 作为用户\n",
    "\n",
    "- 作为生产者\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 14.4 模型与上下文\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 14.5 检测变化\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 15、多体平台系统\n",
    "\n",
    "- 输入：\n",
    "\n",
    "（1）施加的广义力\n",
    "（2）施加的空间力\n",
    "（3）（模型实例的）启动状态\n",
    "（4）几何体查询\n",
    "\n",
    "- 输出：\n",
    "\n",
    "（1）连续状态\n",
    "（2）物体位形\n",
    "（3）物体空间速度\n",
    "（4）物体空间加速度\n",
    "（5）广义加速度\n",
    "（6）反作用力\n",
    "（7）接触状态（结果）\n",
    "（8）（模型实例的）连续状态\n",
    "（9）（模型实例的）广义加速度\n",
    "（10）（模型实例的）广义接触力\n",
    "（11）几何体位形\n",
    "\n",
    "其中几何体查询和几何体位形是与Drake的场景图系统进行交互的端口。\n",
    "\n",
    "多体平台系统提供了面向用户的api：\n",
    "\n",
    "。。。\n",
    "\n",
    "- 模型实例\n",
    "\n",
    "一个多体平台系统可以包含多个模型实例。\n",
    "每个模型实例对应一组物体和它们之间的连接（关节）。\n",
    "模型实例提供了函数方法，用于获取和设置物体状态、连接控制器、以及组织模型等。\n",
    "\n",
    "模型实例通常通过SDF文件进行定义（使用模型标签），通过分析SDF文件自动生成。\n",
    "这里有两个特殊的multibody::ModelInstanceIndex值。\n",
    "世界的值总为0。而1被程序保留。\n",
    "\n",
    "- 系统动态\n",
    "\n",
    "多体平台系统的状态由广义位置和广义速度构成。\n",
    "作为一个Drake系统，多体平台系统方程的形式为$\\dot{x}=f(t,x,u)$。\n",
    "具体的说是\n",
    "\n",
    "$$\n",
    "\\dot{q}=N(q)v \\\\\n",
    "M(q)\\dot{v} + C(q,v) = \\tau\n",
    "$$\n",
    "\n",
    "- 从SDF文件导入模型\n",
    "\n",
    "Drake能从SDF和URDF文件导入多体模型。\n",
    "考虑acrobot的例子\n",
    "\n",
    "~~~\n",
    "MultibodyPlant<T> acrobot;\n",
    "SceneGraph<T> scene_graph;\n",
    "Parser parser(&acrobot, &scene_graph);\n",
    "const std::string relative_name = \"drake/multibody/benchmarks/acrobot/acrobot.sdf\";\n",
    "const std::string full_name = FindResourceOrThrow(relative_name);\n",
    "parser.AddModelFromFile(full_name);\n",
    "~~~\n",
    "\n",
    "就像上面的例子，对于具有视觉几何体、碰撞几何体或二者均存在的模型，用户需要制定一个场景图来处理几何体。\n",
    "可以查看examples/multibody/acrobot/run_lqr.cc的例子\n",
    "\n",
    "AddModelFromFile()可以基于同一个平台被多次调用，这样可以加载多个模型实例。\n",
    "\n",
    "- 使用场景图\n",
    "\n",
    "（1）在框图中增加连接了场景图的多体系统\n",
    "\n",
    "在框图中增加和连接一个带有场景图的多体系统，最简单的方法是使用 AddMultibodyPlantSceneGraph()\n",
    "\n",
    "~~~\n",
    "MultibodyPlant<double>& plant =\n",
    "    AddMultibodyPlantSceneGraph(&builder, 0.0 /* time_step */);\n",
    "plant.DoFoo(...);\n",
    "~~~\n",
    "\n",
    "当不显示需要场景图时，这种方法是最简单的。\n",
    "\n",
    "使用已经声明的指针：\n",
    "\n",
    "~~~\n",
    "MultibodyPlant<double>* plant{};\n",
    "SceneGraph<double>* scene_graph{};\n",
    "std::tie(plant, scene_graph) =\n",
    "    AddMultibodyPlantSceneGraph(&builder, 0.0 /* time_step */);\n",
    "plant->DoFoo(...);\n",
    "scene_graph->DoBar(...);\n",
    "~~~\n",
    "\n",
    "当指针是类成员时，这种方法是最有用的。\n",
    "\n",
    "（2）用场景图注册几何体\n",
    "\n",
    "如果需要可视化和接触建模，多体平台的用户需要使用场景图来注册几何体。\n",
    "\n",
    "在注册几何体之前，用户必须首先调用RegisterAsSourceForSceneGraph()将多体平台注册为场景图实例的客户机（client）。这样，多体平台会得到一个有效的geometry::SourceId。\n",
    "调用 Finalize()时，多体平台需要声明与场景图相关的输入输出端口。\n",
    "而在调用Finalize()之前，必须完成所有几何体的注册。\n",
    "\n",
    "（3）访问点接触参数\n",
    "\n",
    "- 使用多体元素的参数\n",
    "\n",
    "一些多体元素会暴露参数，允许用户灵活的修改平台模型。\n",
    "\n",
    "下面是一个访问和修改刚体质量参数的例子：\n",
    "~~~\n",
    "MultibodyPlant<double> plant;\n",
    "// ... Code to add bodies, finalize plant, and to obtain a context.\n",
    "const RigidBody<double>& body =\n",
    "    plant.GetRigidBodyByName(\"BodyName\");\n",
    "const SpatialInertia<double> M_BBo_B =\n",
    "    body.GetSpatialInertiaInBodyFrame(context);\n",
    "// .. logic to determine a new SpatialInertia parameter for body.\n",
    "const SpatialInertia<double>& M_BBo_B_new = ....\n",
    "// Modify the body parameter for spatial inertia.\n",
    "body.SetSpatialInertiaInBodyFrame(&context, M_BBo_B_new);\n",
    "~~~\n",
    "\n",
    "- 增加模型元素\n",
    "\n",
    "- 接触建模\n",
    "\n",
    "- 能量和驱动\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.11"
  },
  "toc-autonumbering": false,
  "toc-showcode": false,
  "toc-showmarkdowntxt": false,
  "toc-showtags": false
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
